import numpy as np

# 指定文件路径
# file_path = "output.txt"
file_path = "MKFTrajectory_output.txt"

# 给定的刚体坐标系下的变换矩阵
rotation_matrix = np.array([[0.700873705975431, -0.205304197154888, -0.683100457402023],
                           [0.112338918021076, -0.913957993213603, 0.38994968154727],
                           [-0.704383429511314, - 0.350044244734219, - 0.61750547443592]])
translation_vector = np.array(
    [0.215786987214032, -0.198947778789318, -0.00782434870701732])

# 构建位姿矩阵
pose_matrix = np.eye(4)  # 创建一个4x4的单位矩阵
pose_matrix[:3, :3] = rotation_matrix  # 填充旋转矩阵部分
pose_matrix[:3, 3] = translation_vector  # 填充平移向量部分

# 打开文件，以写入模式写入数据
with open(file_path, "w") as file:
    # 读取文件中的数据并生成矩阵
    data_matrix = np.loadtxt("MKFTrajectory.txt")

    # 遍历每个数据
    for item in data_matrix:
        timestamp = item[0]
        x_coord = item[1]
        y_coord = item[2]
        z_coord = item[3]
        x_quat = item[4]
        y_quat = item[5]
        z_quat = item[6]
        w_quat = item[7]
        pose_vector = np.array(
            [x_coord, y_coord, z_coord, x_quat, y_quat, z_quat, w_quat])

        # 将位姿向量转换为齐次坐标
        pose_vector_homogeneous = np.hstack((pose_vector[:3], 1))

        # 进行坐标变换
        pose_transformed_homogeneous = np.dot(
            pose_matrix, pose_vector_homogeneous)

        # 提取变换后的位置和姿态
        transformed_position = pose_transformed_homogeneous[:3]
        transformed_quaternion = pose_vector[3:]

        # 合并为一条信息
        combined_info = [timestamp] + \
            list(transformed_position) + list(transformed_quaternion)

        # 将 combined_info 写入文件
        line = " ".join(str(item) for item in combined_info) + "\n"
        file.write(line)

# 打印保存成功的消息
print("数据已保存到文件:", file_path)